// 独立的串口通信ROS节点
#include "serialport.h"
#include <ros/ros.h>
#include <signal.h>
#include <memory>

class SerialPortNode {
public:
    SerialPortNode() : nh_("~") {
        // 从参数服务器获取配置
        std::string port_name;
        int baud_rate;
        
        nh_.param<std::string>("port", port_name, "/dev/ttyUSB0");
        nh_.param<int>("baud_rate", baud_rate, 115200);
        
        ROS_INFO("启动串口节点: %s, 波特率: %d", port_name.c_str(), baud_rate);
        
        // 创建串口对象
        serial_ = std::make_unique<SerialPort>(port_name, baud_rate);
        
        // 初始化ROS通信
        serial_->initRos(nh_);
        
        // 打开串口
        if (!serial_->openPort()) {
            ROS_ERROR("无法打开串口 %s", port_name.c_str());
            ros::shutdown();
            return;
        }
        
        ROS_INFO("串口节点启动成功!");
        ROS_INFO("订阅话题: /serial/downstream");
        ROS_INFO("发布话题: /serial/upstream/<key_name>");
    }
    
    ~SerialPortNode() {
        if (serial_) {
            serial_->closePort();
        }
    }
    
    void spin() {
        ros::Rate rate(100); // 100Hz
        
        while (ros::ok()) {
            // 处理串口接收数据
            serial_->updateMcuData();
            
            // 处理ROS消息
            ros::spinOnce();
            
            rate.sleep();
        }
    }

private:
    ros::NodeHandle nh_;
    std::unique_ptr<SerialPort> serial_;
};

// 信号处理函数
void signalHandler(int sig) {
    ROS_INFO("接收到退出信号，正在关闭串口节点...");
    ros::shutdown();
}

int main(int argc, char** argv) {
    // 设置ROS中文支持
    setlocale(LC_ALL, "zh_CN.UTF-8");
    
    // 初始化ROS节点
    ros::init(argc, argv, "serialport_node");
    
    // 设置信号处理
    signal(SIGINT, signalHandler);
    signal(SIGTERM, signalHandler);
    
    try {
        // 创建并运行串口节点
        SerialPortNode node;
        node.spin();
    } catch (const std::exception& e) {
        ROS_ERROR("串口节点异常: %s", e.what());
        return -1;
    }
    
    ROS_INFO("串口节点已退出");
    return 0;
}
